/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/* gcc -Wall -O2 -g -c -o slam2D.o slam2D.c */
#include "slam2D.h"

#define NX 3

void slam2D_add_features(
		double *P_new,
		double *P_old,
		int numf,
		int N
		)
{
	
	int i,j;

	for(i=0;i<N;i++)
	{
		for(j=0;j<N;j++)
		{
			P_new[j+i*(N+numf)]=P_old[j+i*N];
		}
	}

	for(i=0;i<numf;i++)
	{
	/*    for(j=0;j<N;j++)
        {
	        P_new[j-i+(N+numf-i-1)*(N+numf)]=0.;
            P_new[N+numf-i-1+(j-i)*(N+numf)]=0.;      
	    }*/   
		P_new[N-i+numf-1+(N+numf-i-1)*(N+numf)]=1.;  
	}

}
void slam2D_pred(
		double *Ppri,
		double *Xpri,
		double *A,
		double *Q,
		int D
		)
{
	double T=1.;
    /* calculate Jacobian and update state */
	calc2D_Xpri(Xpri,A,T,D);
    /* calculate covariance matrix multiplications */
	calc2D_Ppri( Ppri, A, Q, D);    
}
/* this function should allow for different motion models */
void calc2D_Xpri(double *Xpri,double *A, double T, int D)
{
	double x,y,v,theta;
    	int V=4;
	x=Xpri[0];
	y=Xpri[1];
	theta=Xpri[2];
	v=Xpri[3];
	Xpri[0]=x+T*v*cos(theta);
	Xpri[1]=y+T*v*sin(theta);
	/* A=zeros(V,V),V=4 */
	A[0]=1.;
	A[1+V]=1.;
	A[2+2*V]=1.;
	A[3+3*V]=1.;
	A[2*V]=-T*v*sin(theta);
	A[1+2*V]=T*v*cos(theta);
	A[3*V]=T*cos(theta);
	A[1+3*V]=T*sin(theta);

}
void calc2D_Ppri(
		double *P,
		double *A,
		double *Q,
		int N
		)
{
	
	int i,j,k,V=4;
	double AP[4];
/* should use also atlas-lapack here */ 	

	/* vx */
	for(i=0;i<V;i++)
	{
		for(j=0;j<V;j++)
			AP[j]=P[j+N*i];
		for(j=0;j<V;j++)
		{
			P[j+i*N]=A[j]*AP[0];
			for(k=1;k<V;k++)
			{
				P[j+i*N]+=A[j+V*k]*AP[k];
			}
		}
	}
	
	for(i=V;i<N;i++)
	{
		for(j=0;j<V;j++)
			AP[j]=P[j+N*i];
		for(j=0;j<V;j++)
		{
			P[j+i*N]=A[j]*AP[0];
			for(k=1;k<V;k++)
			{
				P[j+i*N]+=A[j+V*k]*AP[k];
			}
			P[i+j*N]=P[j+i*N];
            	}
	}
     /*vv redundant */


	for(i=0;i<V;i++)
	{
		for(j=0;j<V;j++)
			AP[j]=P[i+N*j];
            
		for(j=i;j<V;j++)
		{
			P[i+j*N]=A[j]*AP[0];
			for(k=1;k<V;k++)
			{
				P[i+j*N]+=A[j+V*k]*AP[k];
			}
		}
	}
	
	for(i=0;i<V;i++)
	{
		for(j=i+1;j<V;j++)
		{
			P[j+i*N]=P[i+j*N];
		}
	}


	for(i=0;i<V;i++)
		P[i+i*N]+=Q[i];
}


void calc2D_CC(
		double *CC,
		double *C,
		int N,
		int ind,
		int frame,
		int num_frames)
{
	
	int i,j,base,basep,D,iN;
	D=3;
	base=ind*2+D*num_frames; /* depends number_of_pose parameters and number_of_poses */
	basep=frame*D; /* depends number_of_pose parameters and current_frame */
	/* redundancy here, slow loops */


    
	/*vv C(1:D)=vehicle states, C(1:D)*C(1:D)' */
	for(i=0;i<D;i++)
	{
		iN=(i+basep)*N;
		for(j=0;j<D;j++)
		{
			CC[j+basep+iN]+=C[i]*C[j];
		}
	}
	/*xx   C(D:D+3),CC(base:base+3,base:base+3)=C(base:base+3)*C(base:base+3)'*/
	/*base=(ind-1)*3+D; */
	for(i=0;i<2;i++)
	{
		for(j=0;j<2;j++)
		{
			CC[j+base+(base+i)*N]+=C[i+D]*C[j+D];
		}
	}
	/*vx*/
	for(i=0;i<2;i++)
	{
		for(j=0;j<D;j++)
		{
			CC[j+basep+(i+base)*N]+=C[i+D]*C[j];
		}
	}

	for(i=0;i<D;i++)
	{
		for(j=0;j<2;j++)
		{
			CC[j+base+(i+basep)*N]+=C[i]*C[j+D];
		}
	}
}

void calc2D_Ce(
		double *Ce,
		double *C,
		double *e,
		int ind,
		int frame,
		int num_frames)
{
	
	int j,base,basep,D;
	D=3;

	base=ind*2+D*num_frames; /* depends number_of_pose parameters and number_of_poses */
	basep=frame*D; /* depends number_of_pose parameters and current_frame */
	/* redundancy here, slow loops */
	
	
	

    
	/*vx*/
    
	for(j=0;j<D;j++)
	{
		Ce[basep+j]+=C[j]*e[0];
	}
	for(j=0;j<2;j++)
	{
		Ce[base+j]+=C[j+D]*e[0];
	}
    
}
void calc2D_pose_CC(
		double *CC,
		double *C,
		int N,
		int frame)
{
	
	int i,j,basep,D,iN;
	D=3;
	basep=frame*D; /* depends number_of_pose parameters and current_frame */
	/* redundancy here, slow loops */



    
	/*vv C(1:D)=vehicle states, C(1:D)*C(1:D)' */
	for(i=0;i<D;i++)
	{
		iN=(i+basep)*N;
		for(j=0;j<D;j++)
		{
			CC[j+basep+iN]+=C[i]*C[j];
		}
	}
}

void calc2D_pose_Ce(
		double *Ce,
		double *C,
		double *e,
		int frame
		 )
{
	
	int j,basep,D;
	D=3;

	basep=frame*D; /* depends number_of_pose parameters and current_frame */
	/* redundancy here, slow loops */



    
	/*vx*/
    
	for(j=0;j<D;j++)
	{
		Ce[basep+j]+=C[j]*e[0];
	}
	    
}
/* possible BOOGIE check 2*ind below */
void calc2D_line_CC(
		double *CC,
		double *C,
		int N,
		int ind,
		int frame,
		int num_frames)
{
	
	int i,j,base,basep,D,iN;
	D=3;
	base=ind*2+D*num_frames; /* depends number_of_pose parameters and number_of_poses */
	basep=frame*D; /* depends number_of_pose parameters and current_frame */
	/* redundancy here, slow loops */



    
	/*vv C(1:D)=vehicle states, C(1:D)*C(1:D)' */
	for(i=0;i<D;i++)
	{
		iN=(i+basep)*N;
		for(j=0;j<D;j++)
		{
			CC[j+basep+iN]+=C[i]*C[j];
		}
	}
	/*xx   C(D:D+3),CC(base:base+3,base:base+3)=C(base:base+3)*C(base:base+3)'*/
	/*base=(ind-1)*3+D; */
	for(i=0;i<4;i++)
	{
		for(j=0;j<4;j++)
		{
			CC[j+base+(base+i)*N]+=C[i+D]*C[j+D];
		}
	}
	/*vx*/
	for(i=0;i<4;i++)
	{
		for(j=0;j<D;j++)
		{
			CC[j+basep+(i+base)*N]+=C[i+D]*C[j];
		}
	}

	for(i=0;i<D;i++)
	{
		for(j=0;j<4;j++)
		{
			CC[j+base+(i+basep)*N]+=C[i]*C[j+D];
		}
	}
}

void calc2D_line_Ce(
		double *Ce,
		double *C,
		double *e,
		int ind,
		int frame,
		int num_frames)
{
	
	int j,base,basep,D;
	D=3;

	base=ind*2+D*num_frames; /* depends number_of_pose parameters and number_of_poses */
	basep=frame*D; /* depends number_of_pose parameters and current_frame */
	/* redundancy here, slow loops */

	/* should use atlas-lapack */ 

    
	/*vx*/
    
	for(j=0;j<D;j++)
	{
		Ce[basep+j]+=C[j]*e[0];
	}
	for(j=0;j<4;j++)
	{
		Ce[base+j]+=C[j+D]*e[0];
	}
    
}

void calc2Dlink_CC(
		double *CC,
		int N,
		int ind,
		int frame,
		int num_frames)
{
	
	int i,j,base,basep,D;
	D=3;
	basep=frame*D; /* depends number_of_pose parameters and current_frame */
	/* redundancy here, slow loops */



    
	/*vv C(1:D)=vehicle states, C(1:D)*C(1:D)' */
	for(i=0;i<D;i++)
	{
		
		CC[i+basep+(i+basep)*N]+=1.;

	}
	if(frame>0)
	{
		base=basep-D;
		for(j=0;j<D;j++)
		{
			CC[j+base+(j+base)*N]+=1.;
		}
		for(j=0;j<D;j++)
		{
			CC[j+base+(j+basep)*N]+=-1.;
			CC[j+basep+(j+base)*N]+=-1.;
		}
	}
}

void calc2Dlink_Ce(
		double *Ce,
		double *e,
		int ind,
		int frame,
		int num_frames)
{
	
	int j,base,basep,D;
	D=3;


	basep=frame*D; /* depends number_of_pose parameters and current_frame */
	/* redundancy here, slow loops */



    
	/*vx*/
    
	for(j=0;j<D;j++)
	{
		Ce[basep+j]+=e[j];
	}
	if(frame>0)
	{
		base=basep-D;
		for(j=0;j<D;j++)
		{
			Ce[base+j]+=-e[j];
		}
	}
    
}

void slam2D_ini(
		double *Ppos,
		double *Xpos,
		double *ind,
		double *z,
		double *PC,
		double *K,
		double *R,
		int D,
		int N,
		int num_frames,
		double *er
		)
{
	double e[1];
	int i;
    
	*er=0.0;
	for(i=0;i<num_frames;i++)
	{
		slam2D_ini_iter(Ppos,Xpos,ind,z+i*N,PC,K,R,D,N,i,num_frames,e);
		*er+=*e;
	}
    
    
}
void slam2D_ini_iter(
		double *Ppos,
		double *Xpos,
		double *ind,
		double *z,
		double *PC,
		double *K,
		double *R,
		int D,
		int N,
		int frame,
		int num_frames,
		double *er
		)
{
	int i,indi,DV;
	double e[1],C[5],CPC[1],v[3];
   
	DV=3*num_frames; /*number_of_pose_pars*number_of_poses*/
	for(i=0;i<N;i++) /* for each measurement */
	{
        /* indi is the index to the landmark that corresponds to measurement i*/
		indi=(int)ind[i];
		v[0]=Xpos[frame*3];
		v[1]=Xpos[frame*3+1];
		v[2]=Xpos[frame*3+2];

        /*calculate Jacobian and residual */
		proj2D_fun(e,C,v,z+i,Xpos+DV,indi);
        /*calculate sparse covariance matrix multiplications */
		calc2D_CPC_ini( PC, CPC, R, C, Ppos, D, indi,frame,num_frames);
        /*update covariance and calculate Kalman gain */
		calc2D_PK(Ppos,K,PC,CPC,C,D);
        /*update state	*/
		calc2D_Xpos(Xpos,K,e,D);
        /*cumulative measurement error */
		er[0]+=e[0]*e[0];
        
	}
    
}

void slam2D_bearingmeas(
		double *Ppos,
		double *Xpos,
		double *ind,
		double *z,
		double *PC,
		double *K,
		double *R,
		int D,
		int N,
		double *er
		)
{
	int i,indi;
	double e[1],C[5],CPC[1],v[3];

	for(i=0;i<N;i++) /* for each measurement */
	{
        /* indi is the index to the landmark that corresponds to measurement i*/
		indi=(int)ind[i];
		v[0]=Xpos[0];
		v[1]=Xpos[1];
		v[2]=Xpos[2];

        /*calculate Jacobian and residual */
		proj2D_bearingfun(e,C,v,z+i,Xpos+NX+1,indi);
        /*calculate sparse covariance matrix multiplications */
		calc2D_CPC( PC, CPC, R, C, Ppos, D, indi);
        /*update covariance and calculate Kalman gain */
		calc2D_PK(Ppos,K,PC,CPC,C,D);
        /*update state	*/
		calc2D_Xpos(Xpos,K,e,D);
        /*cumulative measurement error */
		er[0]+=e[0]*e[0];
        
	}
    
}
void proj2D_struct_bearingfun(
		double *e,
		double *J,
		double *pts,
		double *X,
		int ind
		)
{
	double num,ahat,dx,dy;
	
    /* projecting object 2D-coordinates to vehicle frame */

	dx=X[2*ind];
	dy=X[2*ind+1];
	num=dx*dx+dy*dy;
	ahat=atan2(dy,dx);
    
    /*residuals */
  
	e[0]=pts[0]-ahat;
    
	while(e[0]>M_PI)
		e[0]-=2*M_PI;
	while(e[0]<-M_PI)
		e[0]+=2*M_PI;

        
    /* Jacobian */
 
    /*motion parameters for a */

	J[0]=0;
	J[1]=0;
	J[2]=0;

    /*structure parameters for a */
    
	J[3]=-dy/num;
	J[4]=dx/num;
 
}

void proj2D_fun(
			double *e,
			double *J,
			double *v,
			double *pts,
			double *X,
			int ind
			  )
{
	double x,y,phi,num,rhat,ahat,dx,dy;
	
	
	
	x=v[0];
	y=v[1];
	phi=v[2];
    
	/*projecting object 2D-coordinates to vehicle frame*/

	dx=X[2*ind]-x;
	dy=X[2*ind+1]-y;
	num=dx*dx+dy*dy;
	rhat=sqrt(num);
	ahat=atan2(dy,dx)-phi;
    
	/*residuals*/
	e[0]=pts[0]-rhat;
	e[1]=pts[1]-ahat;

    /*BOOGIE? phi(a.k.a v[2]) is not restricted to range [-pi:pi]*/
    

	/*Jacobian*/
	/*motion parameters for r*/
	J[0]=-dx/rhat;
	J[1]=-dy/rhat;
	J[2]=0;
	/*structure parameters for r*/
	J[3]=dx/rhat;
	J[4]=dy/rhat;
	/*motion parameters for a*/
	J[5]=dy/num;
	J[6]=-dx/num;
	J[7]=-1;

	/*structure parameters for a*/
	J[8]=-dy/num;
	J[9]=dx/num;
 
}
/* BOOGIE direction of normal not considered */
void proj2D_linefun(
			double *e,
			double *J,
			double *v,
			double *pts,
			double *X,
			int ind
			  )
{
	double x,y,phi,num,dx,dy;
	double x1,x2,y1,y2,xp,yp,a,b,c,ang;
	double deda,dedb,dedc,dadx1,dadx2,dady1,dady2,dcda,dcdb;
	double dbdx1,dbdx2,dbdy1,dbdy2,dcdx1,dcdx2,dcdy1,dcdy2;
	double dedxp,dedyp,dxpdx,dxpdy,dxpdphi,dypdx,dypdy,dypdphi;
    
	
	
	x=v[0];
	y=v[1];
	phi=v[2];

	/*projecting object 2D-coordinates to vehicle frame*/
	x1=X[2*ind];
	y1=X[2*ind+1];
	x2=X[2*ind+2];
	y2=X[2*ind+3];
	dx=x1-x2;
	dy=y1-y2;/* hmm 2*ind or 4*ind */
	num=sqrt(dx*dx+dy*dy);
	a=dy/num;
	b=-dx/num;
	c=-a*x1-b*y1;
	ang=pts[1]+phi;
	xp=cos(ang)*pts[0]+x;
	yp=sin(ang)*pts[0]+y;
    
    /* BOOGIE? direction of normal not considered (no need?) */
	/*residuals*/
	e[0]=a*xp+b*yp+c;
    
    /*BOOGIE? phi(a.k.a v[2]) is not restricted to range [-pi:pi]*/
    
    /*help variables ? necessary */
    
	dedxp=a;
	dedyp=b;
        
	dxpdphi=-sin(ang)*pts[0];
	dxpdx=1.;
	dxpdy=0.;
    
	dypdphi=cos(ang)*pts[0];
	dypdx=0.;
	dypdy=1.;
    
	deda=xp;
	dedb=yp;
	dedc=1.;
	dadx1=-dx*dy/(num*num*num);
	dadx2=dx*dy/(num*num*num);
	dady1=1./num-dy*dy/(num*num*num);
	dady2=-1./num+dy*dy/(num*num*num);
	dbdx1=-1./num+dx*dx/(num*num*num);
	dbdx2=1./num-dx*dx/(num*num*num);
	dbdy1=dy*dx/(num*num*num);
	dbdy2=-dy*dx/(num*num*num);
	dcda=-x1;
	dcdb=-y1;
	dcdx1=dcda*dadx1+dcdb*dbdx1-a;
	dcdx2=dcda*dadx2+dcdb*dbdx2;
	dcdy1=dcda*dady1+dcdb*dbdy1-b;
	dcdy2=dcda*dady2+dcdb*dbdy2;
    
	/*Jacobian*/
	/*motion parameters*/
	J[0]=dedxp*dxpdx;
	J[1]=dedyp*dypdy;
	J[2]=dedxp*dxpdphi+dedyp*dypdphi;
	/*structure parameters */

	J[3]=deda*dadx1+dedb*dbdx1+dedc*dcdx1;
	J[4]=deda*dady1+dedb*dbdy1+dedc*dcdy1;
	J[5]=deda*dadx2+dedb*dbdx2+dedc*dcdx2;
	J[6]=deda*dady2+dedb*dbdy2+dedc*dcdy2;

}

void proj2D_struct_linefun(
			double *e,
			double *J,
			double *v,
			double *pts,
			double *X,
			int ind
			  )
{
	double x,y,phi,num,dx,dy;
	double x1,x2,y1,y2,xp,yp,a,b,c,ang;
	double deda,dedb,dedc,dadx1,dadx2,dady1,dady2,dcda,dcdb;
	double dbdx1,dbdx2,dbdy1,dbdy2,dcdx1,dcdx2,dcdy1,dcdy2;
   
	
	
	x=v[0];
	y=v[1];
	phi=v[2];
    
	/*projecting object 2D-coordinates to vehicle frame*/
	x1=X[2*ind];
	y1=X[2*ind+1];
	x2=X[2*ind+2];
	y2=X[2*ind+3];
	dx=x1-x2;
	dy=y1-y2;/* hmm 2*ind or 4*ind */
	num=sqrt(dx*dx+dy*dy);
	a=dy/num;
	b=-dx/num;
	c=-a*x1-b*y1;
	ang=pts[1]+phi;
	xp=cos(ang)*pts[0]+x;
	yp=sin(ang)*pts[0]+y;
    
	/*residuals*/
	e[0]=a*xp+b*yp+c;
    
    /*BOOGIE? phi(a.k.a v[2]) is not restricted to range [-pi:pi]*/
    
    /*help variables ? necessary
     */
    
	deda=xp;
	dedb=yp;
	dedc=1.;
	dadx1=-dx*dy/(num*num*num);
	dadx2=dx*dy/(num*num*num);
	dady1=1./num-dy*dy/(num*num*num);
	dady2=-1./num+dy*dy/(num*num*num);
	dbdx1=-1./num+dx*dx/(num*num*num);
	dbdx2=1./num-dx*dx/(num*num*num);
	dbdy1=dy*dx/(num*num*num);
	dbdy2=-dy*dx/(num*num*num);
	dcda=-x1;
	dcdb=-y1;
	dcdx1=dcda*dadx1+dcdb*dbdx1-a;
	dcdx2=dcda*dadx2+dcdb*dbdx2;
	dcdy1=dcda*dady1+dcdb*dbdy1-b;
	dcdy2=dcda*dady2+dcdb*dbdy2;
    
	/*Jacobian*/
	/*motion parameters*/
	J[0]=0;
	J[1]=0;
	J[2]=0;
	/*structure parameters */

	J[3]=deda*dadx1+dedb*dbdx1+dedc*dcdx1;
	J[4]=deda*dady1+dedb*dbdy1+dedc*dcdy1;
	J[5]=deda*dadx2+dedb*dbdx2+dedc*dcdx2;
	J[6]=deda*dady2+dedb*dbdy2+dedc*dcdy2;
       
 
}

void proj2D_pose_linefun(
			double *e,
			double *J,
			double *v,
			double *pts,
			double *X,
			int ind
			  )
{
	double x,y,phi,num,dx,dy;
	double x1,x2,y1,y2,xp,yp,a,b,c,ang;
	double dedxp,dedyp,dxpdx,dxpdy,dxpdphi,dypdx,dypdy,dypdphi;
    
	
	
	x=v[0];
	y=v[1];
	phi=v[2];

	/*projecting object 2D-coordinates to vehicle frame*/
	x1=X[2*ind];
	y1=X[2*ind+1];
	x2=X[2*ind+2];
	y2=X[2*ind+3];
	dx=x1-x2;
	dy=y1-y2;/* hmm 2*ind or 4*ind */
	num=sqrt(dx*dx+dy*dy);
	a=dy/num;
	b=-dx/num;
	c=-a*x1-b*y1;
	ang=pts[1]+phi;
	xp=cos(ang)*pts[0]+x;
	yp=sin(ang)*pts[0]+y;
    
	/*residuals*/
	e[0]=a*xp+b*yp+c;
    
    /*BOOGIE? phi(a.k.a v[2]) is not restricted to range [-pi:pi]*/
    
    /*help variables ? necessary */
    
	dedxp=a;
	dedyp=b;
        
	dxpdphi=-sin(ang)*pts[0];
	dxpdx=1.;
	dxpdy=0.;
    
	dypdphi=cos(ang)*pts[0];
	dypdx=0.;
	dypdy=1.;
      
	/*Jacobian*/
	/*motion parameters*/
	J[0]=dedxp*dxpdx;
	J[1]=dedyp*dypdy;
	J[2]=dedxp*dxpdphi+dedyp*dypdphi;
	/*structure parameters */

/*	J[3]=0.;
	J[4]=0.;
	J[5]=0.;
	J[6]=0.;*/

}

void proj2D_bearingfun(
		double *e,
		double *J,
		double *v,
		double *pts,
		double *X,
		int ind
		)
{
	double x,y,phi,num,ahat,dx,dy;
	
	
	
	x=v[0];
	y=v[1];
	phi=v[2];
    
    /* projecting object 2D-coordinates to vehicle frame */

	dx=X[2*ind]-x;
	dy=X[2*ind+1]-y;
	num=dx*dx+dy*dy;
	ahat=atan2(dy,dx)-phi;
    
    /*residuals */
    
    
	e[0]=pts[0]-ahat;
    
	while(e[0]>M_PI)
        	e[0]-=2*M_PI;
	while(e[0]<-M_PI)
		e[0]+=2*M_PI;
    

        
    /* Jacobian */
 
    /*motion parameters for a */

	J[0]=dy/num;
	J[1]=-dx/num;
	J[2]=-1;

    /*structure parameters for a */
    
	J[3]=-dy/num;
	J[4]=dx/num;

        
 
}

void calc2D_Xpos(
		double *Xpos,
		double *K,
		double *e,
		int N
		)
{
	int j;
    
	for(j=0;j<N;j++)
	{    
		Xpos[j]=Xpos[j]+e[0]*K[j];
	}
}

void calc2D_PK(
		double *P,
		double *K,
		double *PC,
		double *CPC,
		double *C,
		int N
		)
{
	int i,j;
	double KS,iCPC;
	iCPC=1/CPC[0];
 
	for(j=0;j<N;j++)
	{
		K[j]=PC[j]*iCPC;
	}
        
	for(i=0;i<N;i++)
	{
		KS=K[i]*CPC[0];
		for(j=0;j<N;j++)
		{    
			P[j+i*N]=P[j+i*N]-KS*K[j]; 
		}
	}

}

void calc2D_CPC(
		double *PC,
		double *CPC,
		double *R,
		double *C,
		double *P,
		int N,
		int ind)
{
	
	int i,base,iN;
	double CP;

	
	
	/*redundancy here*/
	base=ind*2+4;/*BOOGIE weel not reaaally,just confusing: ehem, there are four vehicle related
	variables, but only three of them affect measurements directly*/

    
	for(i=0,iN=0;i<N;i++,iN+=N)
	{
		PC[i]=C[0]*P[iN];
		PC[i]+=C[1]*P[1+iN];
		PC[i]+=C[2]*P[2+iN];
		PC[i]+=C[3]*P[base+iN];
		PC[i]+=C[4]*P[base+1+iN];
	}
    
    /*	vv */
	CPC[0]=0;
	for(i=0,iN=0;i<3;i++,iN+=N)/* BOOGIE BOOGIE */
	{
		CP=C[0]*P[iN];
		CP+=C[1]*P[1+iN];
		CP+=C[2]*P[2+iN];
		CPC[0]+=C[i]*CP;
	}

    /* xx */
	CPC[0]+=C[3]*C[3]*P[base+N*base]+C[4]*C[4]*P[base+1+N*base+N]+2*C[3]*C[4]*P[base+1+N*base];
    
    /* vx  */
	CP=C[0]*P[N*base];
	CP+=C[1]*P[1+N*base];
	CP+=C[2]*P[2+N*base];

	CPC[0]+=2*C[3]*CP;

	CP=C[0]*P[N*base+N];
	CP+=C[1]*P[1+N*base+N];
	CP+=C[2]*P[2+N*base+N];

	CPC[0]+=2*C[4]*CP;
    
	CPC[0]+=R[0];
}

void calc2D_CPC_ini(
		double *PC,
		double *CPC,
		double *R,
		double *C,
		double *P,
		int N,
		int ind,
		int frame,
		int num_frames)
{
	
	int i,base,basep,iN;
	double CP;

	
	/* should use also atlas-lapack */
	/* should also utilize sparseness */
	/*redundancy here*/
	base=ind*2+3*num_frames; /* depends number_of_pose parameters and number_of_poses */
	basep=frame*3; /* depends number_of_pose parameters and current_frame */
    
	for(i=0,iN=0;i<N;i++,iN+=N)
	{
		PC[i]=C[0]*P[basep+iN];
		PC[i]+=C[1]*P[basep+1+iN];
		PC[i]+=C[2]*P[basep+2+iN];
		PC[i]+=C[3]*P[base+iN];
		PC[i]+=C[4]*P[base+1+iN];
	}
    
    /*	vv */
	CPC[0]=0;
	for(i=0,iN=basep*N;i<3;i++,iN+=N)/* BOOGIE */
	{
		CP=C[0]*P[basep+iN];
		CP+=C[1]*P[basep+1+iN];
		CP+=C[2]*P[basep+2+iN];
		CPC[0]+=C[i]*CP;
	}

    /* xx */
	CPC[0]+=C[3]*C[3]*P[base+N*base]+C[4]*C[4]*P[base+1+N*base+N]+2*C[3]*C[4]*P[base+1+N*base];
    
    /* vx  */
	CP=C[0]*P[basep+N*base];
	CP+=C[1]*P[basep+1+N*base];
	CP+=C[2]*P[basep+2+N*base];

	CPC[0]+=2*C[3]*CP;

	CP=C[0]*P[basep+N*base+N];
	CP+=C[1]*P[basep+1+N*base+N];
	CP+=C[2]*P[basep+2+N*base+N];

	CPC[0]+=2*C[4]*CP;
    
	CPC[0]+=R[0];
}

